﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

using Microsoft.Kinect;
using EngineAccessLayer.Impl;

namespace DepthViewer
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        const float MaxDepthDistance = 4095;
        const float MinDepthDistance = 800;
        const float MaxDepthDistanceOffset = MaxDepthDistance - MinDepthDistance;

        const Int32 BlueIndex = 0;
        const Int32 GreenIndex = 1;
        const Int32 RedIndex = 2;

        const Int32 X = 480;
        const Int32 Y = 240;


        const Int32 X2 = 160;
        const Int32 Y2 = 240;
        const Int32 R = 3;

        DepthProcessor depthProcessor = new DepthProcessor();

        EngineAccess engine;

        List<Int32> listX = new List<Int32>();
        List<Int32> listY = new List<Int32>();
        
        public MainWindow()
        {
            InitializeComponent();

            engine = new EngineAccess(6);
            
            for(Int32 i = 0; i<640; i++ )
            for(Int32 j = 0; j<480; j++ )


                if (near(X, Y, i, j, R) || near(X2, Y2, i, j, R))
                {
                    listX.Add(i);
                    listY.Add(j);
                }
                    
        }


        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            StopKinect(kinectSensorChooser1.Kinect);
        }

        void StopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                sensor.Stop();
                engine.Close();
            }
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            kinectSensorChooser1.KinectSensorChanged += new DependencyPropertyChangedEventHandler(kinectSensorChooser1_KinectSensorChanged);
        }

        void kinectSensorChooser1_KinectSensorChanged(object sender, DependencyPropertyChangedEventArgs e)
        {
            KinectSensor oldSensor = (KinectSensor)e.OldValue;
            StopKinect(oldSensor);


            KinectSensor newSensor = (KinectSensor)e.NewValue;
            newSensor.DepthStream.Enable();
            newSensor.ColorStream.Enable();


            newSensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(newSensor_AllFramesReady);

            try
            {
                newSensor.Start();
            }
            catch (System.IO.IOException)
            {
                kinectSensorChooser1.AppConflictOccurred();
            }

        }

        void newSensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
            {
                if (depthFrame == null)
                {
                    return;
                }
                Byte[] pixels = GenerateColoredBytes(depthFrame);

                Int32 stride = depthFrame.Width * 4;
                depth_image.Source = BitmapSource.Create(depthFrame.Width, depthFrame.Height, 96, 96, PixelFormats.Bgr32, null, pixels, stride);


            }


            
            

        }

        public static Byte CalculateIntensityFromDepth(Int32 distance)
        {
            return (Byte)(255 - (255 * Math.Max(distance - MinDepthDistance, 0) / (MaxDepthDistanceOffset)));
        }

        private Byte[] GenerateColoredBytes(DepthImageFrame depthFrame)
        {
            short[] rawDepthData = new short[depthFrame.PixelDataLength];
            depthFrame.CopyPixelDataTo(rawDepthData);
            Byte[] pixels = new Byte[depthFrame.Height * depthFrame.Width * 4];

            Int32 x;
            Int32 y;
            Int32 mernaTacka = getCount(X, Y, 640);
            Int32 mernaTacka2 = getCount(X2, Y2, 640);

            for (Int32 depthIndex = 0, colorIndex = 0; depthIndex < rawDepthData.Length && colorIndex < pixels.Length; depthIndex++, colorIndex += 4)
            {
                Int32 player = rawDepthData[depthIndex] & DepthImageFrame.PlayerIndexBitmask;
                Int32 depth = rawDepthData[depthIndex] >> DepthImageFrame.PlayerIndexBitmaskWidth;

                Int32 rawDep = rawDepthData[depthIndex];

                Byte intensity = CalculateIntensityFromDepth(depth);
                pixels[colorIndex + BlueIndex] = intensity;
                pixels[colorIndex + GreenIndex] = intensity;
                pixels[colorIndex + RedIndex] = intensity;



                // prilikom bojenja upisuje u text box
                if (colorIndex == mernaTacka)
                {
                    txtDepth.Text = depthProcessor.getHeight(depth).ToString();
                    prbProgres.Value = depthProcessor.getHeight(depth);
                    engine.SetPinHeight(0, double.Parse(txtDepth.Text));
                }

                if (colorIndex == mernaTacka2)
                {
                    textBox1.Text = depthProcessor.getHeight(depth).ToString();
                    progressBar1.Value = depthProcessor.getHeight(depth);
                    engine.SetPinHeight(1, double.Parse(textBox1.Text));
                }


            
            }

            Int32 n = listX.Count;
            for (int i = 0; i < n; i++)
            {

                Int32 ci = getCount(listX[i], listY[i],640);

                pixels[ci + BlueIndex] = 0;
                pixels[ci + GreenIndex] = 0;
                pixels[ci + RedIndex] = 255;

            }
            return pixels;
        }

        private Int32 getY(Int32 count, Int32 width)
        {
            return (Int32)(count / width);
        }

        private Int32 getX(Int32 count, Int32 width, Int32 height)
        {
            return (Int32)(count - width * getY(count, width));
        }

        private Int32 getCount(Int32 X, Int32 Y, Int32 width)
        {
            return Y * width*4 + X*4;
        }

        private bool near(Int32 x1, Int32 y1, Int32 x2, Int32 y2, Int32 r)
        {
            Int32 dist =(Int32) Math.Sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));

            return dist <= r;
        
        }

    }
}
